#include "ros/ros.h"
#include <cstdlib> 
#include <cmath>
#include <cstring>
#include <sstream>
#include <fstream>
// #include <Eigen/Dense>
#include "geometry_msgs/Pose2D.h"

#define PI 3.14159265358979323846
#define M_2PI 6.283185307179586  

using namespace std;

string pgm_file_path,yaml_file_path,occupy_file_path,curt_position_file_path;


bool write_curt_pos_txt(const char* path, int x, int y, int th)
{
  static int num = 0;
  string tt = "x=" + std::to_string(x) + "\ny=" + std::to_string(y) + "\nth=" + std::to_string(th) + "\ntemp=0";
  num = num % 2 + 1;
  string file_name = string(path)  + string("curt_position") + std::to_string(num) + ".txt";
  fstream outfile;
  outfile.open(file_name, std::fstream::in | std::fstream::out);

  if(!outfile)
      {	
	return false;
      }
  
  outfile << tt;
  outfile.close();
  return true;
}

void odomCallback(const geometry_msgs::Pose2D ::ConstPtr &laser_scanner_pose)
{
       //将导航数据信息存储到内部文件中去，方便下次读取
      int x_mm,y_mm,theta_mdeg = 0;
      x_mm = laser_scanner_pose->x * 1000;
      y_mm = laser_scanner_pose->y * 1000;
      theta_mdeg = laser_scanner_pose->theta/ PI *180 * 1000;
      write_curt_pos_txt(curt_position_file_path.c_str(), x_mm, y_mm, theta_mdeg);

}






//čŻťĺéç˝Žćäťśä¸1¤7çĺĺ§ä˝ĺ§żďźç¨äşĺŻźčŞçłťçťçä˝ĺ§żĺĺ§ĺĺč˝
bool read_curt_pos_txt(const char* path, int &x, int &y, int &th) {
    string tt = "";
    bool xbit = false, ybit = false, thbit = false;
    th = 0;
    ifstream infile;
    infile.open(string(path)  + string("curt_position1.txt"), std::ifstream::in);

    if(!infile)
      {
	//printf("Can not open file!\n");
	return false;
      }
    
    while (!infile.eof()) {
        getline(infile, tt);
        //LOG(INFO) << "read curt_position1:" << tt;
        if (tt.find("x=", 0) != tt.npos && tt.length() - 2 > 0) {
            x = stoi(tt.substr(2, tt.length() - 2));
            xbit = true;
        } else if (tt.find("y=", 0) != tt.npos && tt.length() - 2 > 0) {
            y = stoi(tt.substr(2, tt.length() - 2));
            ybit = true;
        } else if (tt.find("th=", 0) != tt.npos && tt.length() > 3 && tt.length() <= 10) {
            th = stoi(tt.substr(3, tt.length() - 3));
            if (th <= 360000 || th >= -360000)
                thbit = true;
        }
    }
    infile.close();
    if (xbit && ybit && thbit) {   
      // cout << "xbit"    <<xbit<<endl; 
      // cout << "ybit"    <<ybit<<endl; 
      // cout << "thbit"    <<thbit<<endl; 
        return true;
    }

    xbit = false;
    ybit = false;
    thbit = false;
    infile.open(string(path)  + string("curt_position1.txt"));
    if(!infile)
      {
	//printf("Can not open file!\n");
	return false;
      }
    while (!infile.eof()) {
        getline(infile, tt);
        //LOG(INFO) << "read curt_position2:" << tt;
        if (tt.find("x=", 0) != tt.npos && tt.length() - 2 > 0) {
            x = stoi(tt.substr(2, tt.length() - 2));
            xbit = true;
        } else if (tt.find("y=", 0) != tt.npos && tt.length() - 2 > 0) {
            y = stoi(tt.substr(2, tt.length() - 2));
            ybit = true;
        } else if (tt.find("th=", 0) != tt.npos && tt.length() > 3 && tt.length() <= 10) {
            th = stoi(tt.substr(3, tt.length() - 3));
            if (th <= 360000 || th >= -360000)
                thbit = true;
        }
    }
    infile.close();
    if (xbit && ybit && thbit) {
              return true;
    } else
        return false;
}






int main(int argc, char** argv)
{  
  ros::init(argc, argv, "initialize_anyway_pkg");

  
  geometry_msgs::Pose2D  laser_scanner_pose;
  ros::NodeHandle nh;
  ros::NodeHandle nh_;
  nh_.getParam("/initialize_anyway_pkg/curt_position_file_path",curt_position_file_path);

  ros::Subscriber sub_basepos = nh.subscribe("odom",1000,odomCallback);
  //nh_.getParam("/relocalization/occupy_file_path",occupy_file_path);
    
  ros::Rate r(100.0);

  while(nh.ok())
  {
    ros::spinOnce();

      int rem_x,rem_y,rem_th;

      bool read_init_pose_flag = read_curt_pos_txt(curt_position_file_path.c_str(), rem_x, rem_y, rem_th);
  
      if(read_init_pose_flag)
	{
	  laser_scanner_pose.x=laser_scanner_pose.x = (double)rem_x / 1000.0;
	  laser_scanner_pose.y = laser_scanner_pose.y = (double)rem_y / 1000.0;
	  laser_scanner_pose.theta = laser_scanner_pose.theta;
	 // laser_scanner_pose.theta = atan2(sin(laser_scanner_pose.theta),cos(laser_scanner_pose.theta));	
    cout <<"laser_scanner_pose.x:"  <<laser_scanner_pose.x<<endl;
    cout <<"laser_scanner_pose.y:"  <<laser_scanner_pose.y<<endl;
    cout <<"laser_scanner_pose.theta:"  <<laser_scanner_pose.theta<<endl;
	}
    
    r.sleep();
  }
  
  return(0);
}